//
// Created by de on 3/30/21.
//

#include "TRunningState.h"
#include "time.h"
TRunningState::TRunningState(StateSelection *p_StateSelection) {

    state = InitState;
    this->p_StateSelection = p_StateSelection;
    p_node = p_StateSelection->getRosHandler();
}


E_State TRunningState::init() {
    return Normal;
}

E_State TRunningState::prepare() {
    return Normal;
}

E_State TRunningState::start() {
//    spdlog::info("TRunningState start running ");
//    std::this_thread::sleep_for(std::chrono::seconds(1));

    VisionMeasure visionMeasure;
    bool Vaild= false;
    double duration_voision=0.0;
    ForceStateData forceCommunData;
    auto t1=std::chrono::system_clock().now();
    try{
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        spdlog::info("启动服务");
        p_node->sendDeviceStart();

        ForceCtlStatus status;
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        bool detect_finish= false;
        do{
            forceCommunData = p_node->getDeviceControlStateStatus();
            status = forceCommunData.status;

            //todo  视觉检测到位  触发力控停止
//            spdlog::info("视觉检测中");
            if(!detect_finish){
                auto t2_1 = std::chrono::system_clock().now();
                visionMeasure = p_node->sendVisionMeaDetect();
                auto t2_2 = std::chrono::system_clock().now();

                duration_voision = std::chrono::duration_cast<std::chrono::milliseconds>(t2_2-t2_1).count()/1000.0;
                if(visionMeasure.CodeMessage == 2000)
                {
                    detect_finish= true;
                    spdlog::info("CodeMessage == 2000");
                    p_node->sendDeviceStop();
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            //装配完毕或视觉检测停止，正常退出
            if(status == Finish){
                visionMeasure.Height=0.0;
                spdlog::info("Finish exit");
                Vaild= true;
                break;
            }
            if(status == Stop) // 正常退出
            {
                spdlog::info(" Stop exit");
                Vaild= true;
                break;
            }
            //系统错误报警退出
            if(status == Err)
            {
                visionMeasure.Height=0.0;
                Vaild= false;
                spdlog::info("Err exit");
                break;
            }

        }while (true);

    }catch (std::exception& e)
    {
        spdlog::error(" error");
        throw PlaformTException(__LINE__,__FILE__, "Mode Error"+std::to_string(state));

    }
    auto t2=std::chrono::system_clock().now();
    double duration_in_ms = std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()/1000.0;
    std::shared_ptr<WorkResult> p_workResult = std::make_shared<WorkResult>(-1, visionMeasure.Height,duration_in_ms,duration_voision,Vaild);
    p_workResult->ErrorMessage =forceCommunData.errCodeInfoData.Err_stringInfo;
    setWorkReult(p_workResult);
    spdlog::info("visionMeasure.Height "+std::to_string(visionMeasure.Height));

    return Normal;
}

E_State TRunningState::reset() {
    return Normal;
}

E_State TRunningState::stop() {
    p_node->sendDeviceStop();
    return Normal;
}

StateStatus TRunningState::getCurrentState() {
    return RunningState;
}

